﻿/*
The MIT License

Copyright (c) 2010 Christoph Husse

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace DARE
{

    /// <summary>
    /// CAstar class
    /// </summary>
    public class CAStar : IPathSearch
    {

    #region identifiers

        /// <summary>
        /// PathNode class
        /// </summary>
        public class PathNode : IPathNode, IComparer<PathNode>
        {

        #region fields

            public static readonly PathNode Comparer = new PathNode(0, 0, true);

            bool m_isWalkable;
            int m_x;
            int m_y;

        #endregion

        #region properties

            // IPathNode impl
            public bool IsWalkable { get { return m_isWalkable; } }
            public int X { get { return m_x; } }
            public int Y { get { return m_y; } }

            // IIndexedObject impl
            public int Index { get; set; }

            public float G { get; internal set; }
            public float H { get; internal set; }
            public float F { get; internal set; }

        #endregion

        #region ctor

            public PathNode(int x, int y, bool isWaklable)
            {
                m_x = x;
                m_y = y;
                m_isWalkable = isWaklable;
            }

        #endregion

        #region methods

            public int Compare(PathNode x, PathNode y)
            {
                if (x.F < y.F)
                    return -1;
                else if (x.F > y.F)
                    return 1;
                return 0;
            }

        #endregion

        }

        /// <summary>
        /// OpenCloseMap class
        /// </summary>
        private class OpenCloseMap
        {

        #region fields

            private PathNode[,] m_Map;
            public int Width { get; private set; }
            public int Height { get; private set; }
            public int Count { get; private set; }

        #endregion

        #region properties

            public PathNode this[Int32 x, Int32 y]
            {
                get { return m_Map[x, y]; }
            }

            public PathNode this[PathNode Node]
            {
                get { return m_Map[Node.X, Node.Y]; }
            }

            public bool IsEmpty
            {
                get { return Count == 0; }
            }

        #endregion

        #region methods

            public OpenCloseMap(int inWidth, int inHeight)
            {
                m_Map = new PathNode[inWidth, inHeight];
                Width = inWidth;
                Height = inHeight;
            }

            public void Add(PathNode inValue)
            {
                PathNode item = m_Map[inValue.X, inValue.Y];

#if DEBUG
#if WINDOWS
                if (item != null)
                    throw new ApplicationException();
#endif
#endif

                Count++;
                m_Map[inValue.X, inValue.Y] = inValue;
            }

            public bool Contains(PathNode inValue)
            {
                PathNode item = m_Map[inValue.X, inValue.Y];

                if (item == null)
                    return false;

#if DEBUG
#if WINDOWS
                if (!inValue.Equals(item))
                    throw new ApplicationException();
#endif
#endif

                return true;
            }

            public void Remove(PathNode inValue)
            {
                PathNode item = m_Map[inValue.X, inValue.Y];

#if DEBUG
#if WINDOWS
                if (!inValue.Equals(item))
                    throw new ApplicationException();
#endif
#endif

                Count--;
                m_Map[inValue.X, inValue.Y] = null;
            }

            public void Clear()
            {
                Count = 0;

                for (int x = 0; x < Width; x++)
                {
                    for (int y = 0; y < Height; y++)
                    {
                        m_Map[x, y] = null;
                    }
                }
            }

        #endregion

        }

    #endregion

    #region fields

        private static readonly float SQRT_2 = (float)Math.Sqrt(2);

        private OpenCloseMap m_ClosedSet;
        private OpenCloseMap m_OpenSet;
        private CPriorityQueue<PathNode> m_OrderedOpenSet;
        private PathNode[,] m_CameFrom;
        private OpenCloseMap m_RuntimeGrid;
        private PathNode[,] m_SearchSpace;
        private LinkedList<IPathNode> m_pathNode;

    #endregion

    #region properties

        public IPathNode[,] SearchSpace { get; private set; }
        public int Width { get; private set; }
        public int Height { get; private set; }
        public LinkedList<IPathNode> Path { get { return m_pathNode; } }
        public bool AllowDiagonalMove { get; set; }

        event EventHandler SearchComplete;
        event EventHandler SearchFailed;

        event EventHandler DARE.IPathSearch.eSearchComplete
        {
            add
            {
                if (SearchComplete != null)
                    lock (SearchComplete)
                    {
                        SearchComplete += value;
                    }
                else
                    SearchComplete = new EventHandler(value);
            }
            remove
            {
                if (SearchComplete != null)
                    lock (SearchComplete)
                    {
                        SearchComplete -= value;
                    }
            }
        }

        event EventHandler DARE.IPathSearch.eSearchFailed
        {
            add
            {
                if (SearchFailed != null)
                    lock (SearchFailed)
                    {
                        SearchFailed += value;
                    }
                else
                    SearchFailed = new EventHandler(value);
            }
            remove
            {
                if (SearchFailed != null)
                    lock (SearchFailed)
                    {
                        SearchFailed -= value;
                    }
            }
        }

    #endregion

    #region ctor

        public CAStar()
        {
            AllowDiagonalMove = true;
        }

    #endregion

    #region methods

        public void SetMap(int[,] map)
        {
            Width = map.GetLength(0);
            Height = map.GetLength(1);
            m_SearchSpace = new PathNode[Width, Height];
            SearchSpace = new IPathNode[Width, Height];
            m_ClosedSet = new OpenCloseMap(Width, Height);
            m_OpenSet = new OpenCloseMap(Width, Height);
            m_CameFrom = new PathNode[Width, Height];
            m_RuntimeGrid = new OpenCloseMap(Width, Height);
            m_OrderedOpenSet = new CPriorityQueue<PathNode>(PathNode.Comparer);

            for (int x = 0; x < Width; x++)
                for (int y = 0; y < Height; y++)
                {
                    if (map[x, y] == null)
                        throw new ArgumentNullException();
                    m_SearchSpace[x, y] = new PathNode(x, y, map[x, y] == 0);
                    SearchSpace[x, y] = new PathNode(x, y, map[x, y] == 0);
                }
        }

        public void SetMap(IPathNode[,] map)
        {
            SearchSpace = map;
            Width = map.GetLength(0);
            Height = map.GetLength(1);
            m_SearchSpace = new PathNode[Width, Height];
            m_ClosedSet = new OpenCloseMap(Width, Height);
            m_OpenSet = new OpenCloseMap(Width, Height);
            m_CameFrom = new PathNode[Width, Height];
            m_RuntimeGrid = new OpenCloseMap(Width, Height);
            m_OrderedOpenSet = new CPriorityQueue<PathNode>(PathNode.Comparer);

            for (int x = 0; x < Width; x++)
                for (int y = 0; y < Height; y++)
                {
                    if (map[x, y] == null)
                        throw new ArgumentNullException();
                    m_SearchSpace[x, y] = new PathNode(x, y, map[x, y].IsWalkable);
                }
        }

        protected virtual float Heuristic(PathNode inStart, PathNode inEnd)
        {
            return (float)Math.Sqrt((inStart.X - inEnd.X) * (inStart.X - inEnd.X) + (inStart.Y - inEnd.Y) * (inStart.Y - inEnd.Y));
        }

        protected virtual float NeighborDistance(PathNode inStart, PathNode inEnd)
        {
            int diffX = Math.Abs(inStart.X - inEnd.X);
            int diffY = Math.Abs(inStart.Y - inEnd.Y);

            switch (diffX + diffY)
            {
                case 1: return 1;
                case 2: return SQRT_2;
                case 0: return 0;
            }
            return Heuristic(inStart, inEnd);
        }

        /// <summary>
        /// Returns null, if no path is found. Start- and End-Node are included in returned path. The user context
        /// is passed to IsWalkable().
        /// </summary>
        public void Search(Object way)
        {
            if (way == null)
                throw new ArgumentNullException();

            m_pathNode = null;
            CWay w = (CWay)way;
            PathNode startNode = m_SearchSpace[w.sX, w.sY];
            PathNode endNode = m_SearchSpace[w.eX, w.eY];

            if (startNode == endNode)
                return;

            PathNode[] neighborNodes = new PathNode[8];

            m_ClosedSet.Clear();
            m_OpenSet.Clear();
            m_RuntimeGrid.Clear();
            m_OrderedOpenSet.Clear();

            for (int x = 0; x < Width; x++)
                for (int y = 0; y < Height; y++)
                    m_CameFrom[x, y] = null;

            startNode.G = 0;
            startNode.H = Heuristic(startNode, endNode);
            startNode.F = startNode.H;

            m_OpenSet.Add(startNode);
            m_OrderedOpenSet.Push(startNode);
            m_RuntimeGrid.Add(startNode);

            int nodes = 0;

            while (!m_OpenSet.IsEmpty)
            {
                PathNode x = m_OrderedOpenSet.Pop();

                if (x == endNode)
                {
                    m_pathNode = ReconstructPath(m_CameFrom, m_CameFrom[endNode.X, endNode.Y]);
                    m_pathNode.AddLast(endNode);
                    if (SearchComplete != null)
                        SearchComplete(this, null);
                    return;
                }

                m_OpenSet.Remove(x);
                m_ClosedSet.Add(x);
                StoreNeighborNodes(x, neighborNodes);

                for (int i = 0; i < neighborNodes.Length; i++)
                {
                    PathNode y = neighborNodes[i];
                    Boolean tentative_is_better;

                    if (y == null)
                        continue;
                    if (!y.IsWalkable)
                        continue;
                    if (m_ClosedSet.Contains(y))
                        continue;

                    nodes++;
                    float tentative_g_score = m_RuntimeGrid[x].G + NeighborDistance(x, y);
                    Boolean wasAdded = false;

                    if (!m_OpenSet.Contains(y))
                    {
                        m_OpenSet.Add(y);
                        tentative_is_better = true;
                        wasAdded = true;
                    }
                    else if (tentative_g_score < m_RuntimeGrid[y].G)
                        tentative_is_better = true;
                    else
                        tentative_is_better = false;

                    if (tentative_is_better)
                    {
                        m_CameFrom[y.X, y.Y] = x;

                        if (!m_RuntimeGrid.Contains(y))
                            m_RuntimeGrid.Add(y);

                        m_RuntimeGrid[y].G = tentative_g_score;
                        m_RuntimeGrid[y].H = Heuristic(y, endNode);
                        m_RuntimeGrid[y].F = m_RuntimeGrid[y].G + m_RuntimeGrid[y].H;

                        if (wasAdded)
                            m_OrderedOpenSet.Push(y);
                        else
                            m_OrderedOpenSet.Update(y);
                    }
                }
            }
            if (SearchFailed != null)
                SearchFailed(this, null);
        }

        private LinkedList<IPathNode> ReconstructPath(PathNode[,] came_from, PathNode current_node)
        {
            LinkedList<IPathNode> result = new LinkedList<IPathNode>();

            ReconstructPathRecursive(came_from, current_node, result);
            return result;
        }

        private void ReconstructPathRecursive(PathNode[,] came_from, PathNode current_node, LinkedList<IPathNode> result)
        {
            PathNode item = came_from[current_node.X, current_node.Y];

            if (item != null)
            {
                ReconstructPathRecursive(came_from, item, result);
                result.AddLast(current_node);
            }
            else
                result.AddLast(current_node);
        }

        private void StoreNeighborNodes(PathNode inAround, PathNode[] inNeighbors)
        {
            int x = inAround.X;
            int y = inAround.Y;

            if (y > 0)             inNeighbors[1] = m_SearchSpace[x, y - 1];
            else                   inNeighbors[1] = null;

            if (x > 0)             inNeighbors[3] = m_SearchSpace[x - 1, y];
            else                   inNeighbors[3] = null;

            if (x < Width - 1)     inNeighbors[4] = m_SearchSpace[x + 1, y];
            else                   inNeighbors[4] = null;

            if (y < Height - 1)    inNeighbors[6] = m_SearchSpace[x, y + 1];
            else                   inNeighbors[6] = null;

            if (AllowDiagonalMove)
            {
                if ((x > 0) && (y > 0))                  inNeighbors[0] = m_SearchSpace[x - 1, y - 1];
                else                                     inNeighbors[0] = null;

                if ((x > 0) && (y < Height - 1))         inNeighbors[5] = m_SearchSpace[x - 1, y + 1];
                else                                     inNeighbors[5] = null;

                if ((x < Width - 1) && (y > 0))          inNeighbors[2] = m_SearchSpace[x + 1, y - 1];
                else                                     inNeighbors[2] = null;

                if ((x < Width - 1) && (y < Height - 1)) inNeighbors[7] = m_SearchSpace[x + 1, y + 1];
                else                                     inNeighbors[7] = null;
            }
        }

    #endregion

    }
}
